#ifndef TRAJECTORY_REPLAN_H
#define TRAJECTORY_REPLAN_H

#include <ros/ros.h>
//for trajectory planning
#include <airsim_ros/DesiredStates.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <airsim_ros/Takeoff.h>
#include <airsim_ros/VelData.h>
#include <airsim_ros/CirclePoses.h>
#include <airsim_ros/MinijerkTricker.h>

// circe detect
#include <custom_msgs/CircleRecog.h>

//for cv
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

#include <memory>
#include <vector>
#include <Eigen/Eigen>

#include "trajectory_generator/mini_jerk_traj.h"

using namespace std;
using namespace cv;

class TrajectoryReplanNode {


public:
    TrajectoryReplanNode(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private);
    ~TrajectoryReplanNode();

    enum STATE {
        INIT = 0,
        TAKEOFF,
        PASS_CIRCLE_REPLAN,
        FLY_TO_CIRCLE_PLAN,
        PUB_TRAJ
    };

    inline Eigen::Vector3d transformPixel2World(const Eigen::Vector3d &pixel_and_depth) {
        Eigen::Vector3d point_p (pixel_and_depth(0)*pixel_and_depth(2),
                                 pixel_and_depth(1)*pixel_and_depth(2),
                                 pixel_and_depth(2));
        //坐标变换 像素坐标系point_p 相机坐标系point_c 世界坐标系point_w
        //cout << "ceicle Center in pixel:" << point_p.transpose() << endl;                         
        // Eigen::Matrix3d K; //内参矩阵
        // K << 160.0, 0.0, 160.0, 0.0, 160.0, 120.0, 0.0, 0.0, 1.0;
        Eigen::Matrix3d K_inverse;
        K_inverse<<0.00625, 0.0, -1.0, 0.0, 0.00625, -0.75, 0.0, 0.0, 1.0;
        //Eigen::Vector3d point_c = K.inverse() * point_p;
        Eigen::Vector3d point_c = K_inverse * point_p;
        // cout << "K.inverse OKK!" << endl;

        Eigen::Matrix3d R_c_b;
        R_c_b << 0, 0, 1, 1, 0, 0, 0, 1, 0;
        //cout << R_c_b << endl;
        Eigen::Vector3d t_c_b (0.26, 0, 0);
        Eigen::Vector3d point_b = R_c_b * point_c + t_c_b;
        //cout << "ceicle Center in body:" << point_b.transpose() << endl;
        Eigen::Matrix3d R_b_w = odom_orient_.normalized().toRotationMatrix();
        //Eigen::Matrix3d R_b_w = Eigen::Matrix3d::Identity();
        Eigen::Vector3d t_b_w = odom_pos_;
        Eigen::Vector3d point_w = R_b_w * point_b + t_b_w;
        //cout << "uav current pos:" << t_b_w.transpose() << endl;
        // Eigen::Quaterniond q = odom_orient_.normalized();
        // Eigen::Isometry3d T_wb(q);
        // T_wb.pretranslate(odom_pos_);
        // Eigen::Vector3d point_w = T_wb * point_b;
        // cout << "ceicle Center in world:" << point_w.transpose() << endl;
        //cout << "distance to center:" << (point_w - odom_pos_).norm() << endl;
        return point_w;
    }

    void odomCallback(const nav_msgs::OdometryConstPtr &msg);
    void posCallback(const geometry_msgs::PoseStampedConstPtr &msg);
    void velCallback(const airsim_ros::VelDataConstPtr &msg);
    void circlePosCallback(const airsim_ros::CirclePosesConstPtr &msg);
    void minijerkTrickerCallback(const airsim_ros::MinijerkTrickerConstPtr &msg);
    Eigen::VectorXd timeAllocation(const Eigen::MatrixXd &waypoints);
    bool trajectoryGenerate(const Eigen::MatrixXd &waypoints);
    void FSMProcess(const ros::TimerEvent &e);
    void circleCheckProcess(const ros::TimerEvent &e);
    void desiredStatesPub();
    void displayTrajWithColor();
    void drawCmd(const Eigen::Vector3d& pos, const Eigen::Vector3d& vec, const int& id,
                 const Eigen::Vector4d& color);
    //get depth image and get circle center
    void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);
    // void getCircleCenter(const ros::TimerEvent &e);
    // void findcircle(const darknet_ros_msgs::BoundingBoxes::ConstPtr& a);
    void setWaypointsForMinijerk(Eigen::Vector3d start_p, Eigen::Vector3d end_p);
    void circleDetectCallback(const custom_msgs::CircleRecogConstPtr& msg);
    void changeFSMstate(STATE state);

private:
    ros::NodeHandle nh_;
    ros::NodeHandle nh_private_;
    //for trajectory planning
    ros::Publisher desiredStates_pub_, traj_vis_pub_, cmd_vis_pub_;
    ros::Publisher desiredPose_pub_, currentPose_pub_;
    ros::Subscriber odom_sub_;
    ros::Subscriber vel_sub_;
    ros::Subscriber pos_sub_;  
    ros::Subscriber circlePos_sub_;
    ros::Subscriber minijerk_tricker_sub_;
    ros::Subscriber circle_detect_sub_;
    ros::Timer traj_proc_timer_;
    ros::Timer circle_check_timer_;
    airsim_ros::DesiredStates cmd_;
    // nav_msgs::OdometryConstPtr odom_;
    geometry_msgs::PoseStampedConstPtr pos_;
    geometry_msgs::PoseStamped desiredPose_, currentPose_;
    ros::ServiceClient takeoff_client_;
    airsim_ros::MinijerkTrickerConstPtr minijerk_tricker_info_;

    //YOLO
    ros::Subscriber circle_info;
    bool circle_info_init = true;

    bool minijerk_tricker_ = false;
    bool trajectoryOK_ = false;
    bool first_minijerk_ = true;
    
    //parameters for trajectory planning
    Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_;
    Eigen::Quaterniond odom_orient_;
    Eigen::Vector3d desired_pos_;
    Eigen::Vector3d dir_;
    // 当前圆心世界坐标
    Eigen::Vector3d circle_target_pos_;
    // 记录圆心的序号
    int circle_count_ = 0;
    // 检测到的圆心世界坐标
    Eigen::Vector3d circle_detect_pos_;
    // 检测到的圆心法向量
    Eigen::Vector3d circle_detect_normal_vec_;
    // 判断穿越圆环
    Eigen::Vector3d target_v_pre_;
    Eigen::Vector3d target_v_;
    Eigen::VectorXd times_;
    Eigen::MatrixXd coeff_matrix_;
    Eigen::MatrixXd waypoints_;
    Eigen::MatrixXd circle_poses_; // 全部圆的近似世界坐标
    int circle_num_;
    int waypoint_num_;
    double max_vel_, max_acc_, max_jerk_, scalar_1, scalar_2;
    ros::Time start_time_ ;
    ros::Time final_time_ ;
    int segment_num_;
    double traj_duration_;
    double start_yaw_, final_yaw_;
    bool got_circle_flag_;
    int row_idx_ = 1;

    //圆心坐标 x y
    double biggestBox_center[2];
    double time_sum_ = 0.0;
    int trajectory_count_ = 0;

    double end_vel_x;
    double end_vel_y;
    double end_vel_z;

    // FSM
    STATE fsm_state;
    STATE last_fsm_state;



    //boundingbox xmin xmax ymin ymax
    int boundingbox_xmin, boundingbox_xmax, boundingbox_ymin, boundingbox_ymax;

    int check_step = 1;

    //for cv
    std::unique_ptr<image_transport::ImageTransport> it_;
    image_transport::Subscriber color_image_sub_;
    image_transport::Subscriber depth_image_sub_;
    cv_bridge::CvImageConstPtr color_ptr_, depth_ptr_;
    vector<vector<Point>> pt_;
    
    shared_ptr<TrajectoryGeneratorWaypoints> trajPlanWaypoints_ = make_shared<TrajectoryGeneratorWaypoints>();

};

#endif